home *** CD-ROM | disk | FTP | other *** search
/ SGI Developer Toolbox 6.1 / SGI Developer Toolbox 6.1 - Disc 1.iso / toolbox / src / demos / OpenGL / walker / walker.c < prev    next >
C/C++ Source or Header  |  1996-11-11  |  28KB  |  1,106 lines
  1. #include <GL/gl.h>
  2. #include <GL/glu.h>
  3. #include <GL/glut.h>
  4. #include <stdio.h>
  5. #include <stdlib.h>
  6. #include <math.h>
  7. #include <string.h>
  8. #include <dirent.h>
  9. #include <assert.h>
  10.  
  11. #include "walker.h"
  12.  
  13.  
  14.  
  15. #define MAX(x,y) ((x) > (y) ? (x) : (y))
  16. #define MIN(x,y) ((x) < (y) ? (x) : (y))
  17.  
  18. #define CYCLE_SIZE 100
  19. #define CYCLE_STEP 1.0/CYCLE_SIZE
  20. #define OVERSAMPLE 10
  21.  
  22. #define MAX_CPOINTS 34   /* 2 end point ones and 10 in the middle */
  23.  
  24. #define MAX_CSETNAMELEN 25
  25.  
  26. #define NUM_JOINTS 5
  27.  
  28. #define CSET_EXT ".cset"
  29. #define CSET_EXTLEN 5
  30.  
  31. #include "walkviewer.h"
  32.  
  33.  
  34. typedef enum { CMENU_QUIT, CMENU_CURVE, CMENU_HORIZ, CMENU_RESET,
  35.                CMENU_WALK, CMENU_DONEEDIT, CMENU_SAVE, CMENU_LOAD,
  36.            CMENU_MIRROR
  37.               } CurveMenuChoices;
  38.  
  39. typedef enum { WIRECUBE, SOLIDCUBE, CYLINDER1, CYLINDER2 } ModelTypes;
  40.  
  41. int GuyModel = SOLIDCUBE;
  42.  
  43. /***************************************************************/
  44. /*************************** GLOBALS ***************************/
  45. /***************************************************************/
  46.  
  47. GLuint HorizontalList, AxesList,     /* Display lists */
  48.        CurveLists, ControlPtsLists;  /* Firsts of groups of display lists */
  49.  
  50. int CurveAsPoints   =  0,    /* Display curve as points? */
  51.     DrawHorizontals =  0,    /* Draw horizontal lines?   */
  52.     EditingCurve    = -1;    /* Editing what curve, -1 means none */
  53.  
  54. int CurveWindow,   /* Glut window id's to two top level windows */
  55.     GuyWindow;
  56.  
  57. int MirrorLegs = 0;
  58.  
  59. int CurveMenu     = -1,
  60.     CurveEditMenu = -1,
  61.     StepSizeMenu  = -1,
  62.     LoadMenu      = -1,
  63.     SaveMenu      = -1;
  64.  
  65. char *CSetNames[MAX_CSETNAMELEN];
  66.  
  67. int CurrentCurve = -1;  /* Curve loaded, index to CSetNames */
  68.  
  69. GLfloat Walk_cycle[2][NUM_JOINTS][CYCLE_SIZE];  /* array of computed angles */
  70.  
  71. int Step = CYCLE_SIZE/2;  /* Position in cycle, start in middle */
  72.  
  73. float fStep = CYCLE_SIZE/2;   /* floating point for non-integer steping */
  74. float IncStep = 1.0;
  75.  
  76. typedef struct ControlPts {
  77.   int numpoints;
  78.   float xcoords[MAX_CPOINTS];
  79.   float angles[MAX_CPOINTS];
  80. } tControlPts;
  81.  
  82. tControlPts RotCurve[NUM_JOINTS];   /* series of cntrl points for ea joint */
  83.  
  84. int Walking         = 0,     /* Guy is walking? */
  85.     ViewPerspective = 1,     /* Perspective or orthographic views */
  86.     DrawAxes        = 0;     /* Draw axes for alignment */
  87.  
  88. int CurveWWidth,             /* Dimensions of curve window */
  89.     CurveWHeight;  
  90.  
  91. int CurveDownBtn = -1,               /* mouse stuff, for editing curves */
  92.     WasWalking,
  93.     CurvePickedPoint = -1,
  94.     CurveLastX,
  95.     CurveLastY;
  96.  
  97. int CurveWindowVisible = 1;
  98.  
  99.  
  100.   /* prototypes */
  101. void RedisplayBoth(void);
  102. void IncrementStep(void);
  103. void CurveCPointDrag(void);
  104. void CurveHandleMenu(int value);
  105. void StopWalking(void);
  106. void CurveHandleEditMenu(int curve);
  107. void ComputeCSetAndMakeLists(void);
  108. int MakeLoadAndSaveMenus(void);
  109. void CurveMenuInit(void);
  110. void SetWindowTitles(char *csetname);
  111.  
  112. /***************************************************************/
  113. /**************************** BEZIER ***************************/
  114. /***************************************************************/
  115.  
  116.   /* Matrix times a vector  dest = m*v */
  117. void MultMV(float m[3][4], float v[4], float dest[3])
  118. {
  119.   int i, j;
  120.  
  121.   for (i = 0; i < 3; i++) {
  122.     dest[i] = 0;
  123.     for (j = 0; j < 4; j++)
  124.       dest[i] += m[i][j] * v[j];
  125.   }
  126. }
  127.  
  128.   /* Matrix multiplication, dest = m1*m2 */
  129. void MultM(float m1[3][4], float m2[4][4], float dest[3][4])
  130. {
  131.   int i, j, k;
  132.  
  133.   for (i = 0; i < 3; i++)
  134.     for (j = 0; j < 4; j++) {
  135.       dest[i][j] = 0;
  136.       for (k = 0; k < 4; k++)
  137.       dest[i][j] += (m1[i][k] * m2[k][j]);
  138.     }
  139. }
  140.  
  141. void ComputeCurve(int joint)
  142. {
  143.   float prod[3][4], tm[4], pos[3];
  144.   float t = 0, tinc = (float)CYCLE_STEP/OVERSAMPLE;
  145.   int ctlpoint, i;
  146.   float BBasis[4][4] = {{-1, 3, -3, 1}, {3, -6, 3, 0},
  147.                         {-3, 3,  0, 0}, {1,  0, 0, 0}};
  148.   int lastindex, newindex;
  149.  
  150.   float pointset[3][4];
  151.   for (i = 0; i < 4; i++)   /* z's are always zero, only 2-d */
  152.     pointset[2][i] = 0; 
  153.  
  154.   lastindex = -1;
  155.   for(ctlpoint = 0; ctlpoint < RotCurve[joint].numpoints; ctlpoint += 3) {
  156.     t = 0;
  157.     for (i = 0; i < 4; i++)
  158.       pointset[0][i] = RotCurve[joint].xcoords[ctlpoint + i];
  159.     for (i = 0; i < 4; i++)
  160.       pointset[1][i] = RotCurve[joint].angles[ctlpoint + i];
  161.  
  162.     MultM(pointset, BBasis, prod);
  163.  
  164.     while (t <= 1) {
  165.       tm[0] = t*t*t;
  166.       tm[1] = t*t;
  167.       tm[2] = t;
  168.       tm[3] = 1;
  169.       MultMV(prod, tm, pos);
  170.       newindex = (int)(pos[0]*(CYCLE_SIZE-1));
  171.       if ((int)(newindex > lastindex))  {  /* go at least one */
  172.     Walk_cycle[0][joint][newindex] = pos[1];
  173.     lastindex++;
  174.       } 
  175.       t += tinc;
  176.     }
  177.   }
  178.  
  179.   for (i = 0; i < CYCLE_SIZE; i++) {      /* copy to other leg, out-o-phase */
  180.     if (MirrorLegs)
  181.       Walk_cycle[1][joint][i] =
  182.         Walk_cycle[0][joint][i];
  183.     else
  184.       Walk_cycle[1][joint][i] =
  185.         Walk_cycle[0][joint][(i+(CYCLE_SIZE/2))%CYCLE_SIZE];
  186.   }
  187. }
  188.  
  189. /***************************************************************/
  190. /************************* CURVE I/O ***************************/
  191. /***************************************************************/
  192.  
  193. void FlatCSet(void)
  194. {
  195.   int joint;
  196.  
  197.   for (joint = 0; joint < NUM_JOINTS; joint++) {
  198.     RotCurve[joint].numpoints = 4;
  199.     RotCurve[joint].xcoords[0] = 0.0;
  200.     RotCurve[joint].angles[0]  = 0.0;
  201.     RotCurve[joint].xcoords[1] = 0.2;
  202.     RotCurve[joint].angles[1]  = 0.0;
  203.     RotCurve[joint].xcoords[2] = 0.8;
  204.     RotCurve[joint].angles[2]  = 0.0;
  205.     RotCurve[joint].xcoords[3] = 1.0;
  206.     RotCurve[joint].angles[3]  = 0.0;
  207.   }
  208. }
  209.  
  210. int ReadCSetFromFile(char *filename)
  211. {
  212.   FILE *infile = fopen(filename, "r");
  213.   int numjoints, numpoints, joint, point, mirrorlegs;
  214.   float value;
  215.  
  216.   if (infile == NULL)
  217.     goto abort;
  218.   
  219.   if (!fscanf(infile, " %d", &numjoints) || numjoints != NUM_JOINTS)
  220.     goto abort;
  221.  
  222.   if (!fscanf(infile, " %d", &mirrorlegs) || (mirrorlegs != 0 &&
  223.                                               mirrorlegs != 1))
  224.     goto abort;
  225.  
  226.   MirrorLegs = mirrorlegs;
  227.  
  228.   for (joint = 0; joint < NUM_JOINTS; joint++) {
  229.     if (!fscanf(infile, " %d", &numpoints) || numpoints < 4 || 
  230.                                               numpoints > MAX_CPOINTS)
  231.       goto abort;
  232.     RotCurve[joint].numpoints = numpoints;
  233.     for (point = 0; point < numpoints; point++) {
  234.       if (!fscanf(infile, " %f", &value))
  235.     goto abort;
  236.       RotCurve[joint].xcoords[point] = value;
  237.     }
  238.     for (point = 0; point < numpoints; point++) {
  239.       if (!fscanf(infile, " %f", &value))
  240.     goto abort;
  241.       RotCurve[joint].angles[point] = value;
  242.     }
  243.   }
  244.  
  245.   fclose(infile);
  246.   return 0;
  247.  
  248.   abort:
  249.     fclose(infile);
  250.     fprintf(stderr, "Something went wrong while reading file %s\n", filename);
  251.     FlatCSet();
  252.     return -1;
  253. }
  254.  
  255. void WriteCSetToFile(char *filename)
  256. {
  257.   FILE *outfile = fopen(filename, "w+");
  258.   int joint, point;
  259.  
  260.   if (outfile == NULL) {
  261.     fprintf(stderr, "Error: could not create file %s\n", filename);
  262.     return;
  263.   }
  264.  
  265.   fprintf(outfile, "%d\n", NUM_JOINTS);
  266.  
  267.   fprintf(outfile, "%d\n", MirrorLegs);
  268.  
  269.   for (joint = 0; joint < NUM_JOINTS; joint++) {
  270.  
  271.     fprintf(outfile, "%d\n", RotCurve[joint].numpoints);
  272.  
  273.     for (point = 0; point < RotCurve[joint].numpoints; point++) {
  274.       fprintf(outfile, "%f ", RotCurve[joint].xcoords[point]);
  275.     }
  276.     fprintf(outfile, "\n");
  277.  
  278.     for (point = 0; point < RotCurve[joint].numpoints; point++) {
  279.       fprintf(outfile, "%f ", RotCurve[joint].angles[point]);
  280.     }
  281.     fprintf(outfile, "\n");
  282.   }
  283.  
  284.   fclose(outfile);
  285. }
  286.  
  287. void HandleLoadMenu(int cset)
  288. {
  289.   char filename[MAX_CSETNAMELEN + CSET_EXTLEN + 1];
  290.  
  291.   if (cset == -1) {
  292.     MakeLoadAndSaveMenus();
  293.     CurveMenuInit();
  294.   } else {
  295.     (void)strcpy(filename, CSetNames[cset]);
  296.     (void)strcat(filename, CSET_EXT);
  297.     if (ReadCSetFromFile(filename) == 0) {
  298.       glutSetMenu(SaveMenu);
  299.       glutChangeToMenuEntry(1, CSetNames[cset], cset);
  300.       ComputeCSetAndMakeLists();
  301.       SetWindowTitles(CSetNames[cset]);
  302.       RedisplayBoth();
  303.     }
  304.   }
  305. }
  306.  
  307. void HandleSaveMenu(int cset)
  308. {
  309.   char filename[MAX_CSETNAMELEN + CSET_EXTLEN + 1];
  310.  
  311.   (void)strcpy(filename, CSetNames[cset]);
  312.   (void)strcat(filename, CSET_EXT);
  313.   WriteCSetToFile(filename);
  314.   ComputeCSetAndMakeLists();
  315.   RedisplayBoth();
  316. }
  317.  
  318. int MakeLoadAndSaveMenus(void)
  319. {
  320.   DIR *dirp = opendir(".");
  321.   struct dirent *direntp;
  322.   int csetnum = 0;
  323.   char *newcsetname;
  324.  
  325.   if (LoadMenu != -1)
  326.     glutDestroyMenu(LoadMenu);
  327.   if (SaveMenu != -1)
  328.     glutDestroyMenu(SaveMenu);
  329.  
  330.   SaveMenu = glutCreateMenu(HandleSaveMenu);
  331.   LoadMenu = glutCreateMenu(HandleLoadMenu);
  332.  
  333.   if (dirp == NULL) {
  334.     fprintf(stderr, "Error opening current dir in MakeLoadAndSaveMenus\n");
  335.     return(0);
  336.   }
  337.  
  338.   while ((direntp = readdir(dirp)) != NULL) {
  339.     char *ext = direntp->d_name + (strlen(direntp->d_name) - CSET_EXTLEN);
  340.     if (!strcmp(ext, CSET_EXT)) {
  341.       newcsetname = malloc(strlen(direntp->d_name) - CSET_EXTLEN + 1);
  342.       strncpy(newcsetname, direntp->d_name,
  343.           strlen(direntp->d_name) - CSET_EXTLEN);
  344.       newcsetname[strlen(direntp->d_name) - CSET_EXTLEN] = 0;
  345.       CSetNames[csetnum] = newcsetname;
  346.       glutAddMenuEntry(newcsetname, csetnum++);
  347.     }
  348.   }
  349.   closedir(dirp);
  350.   glutSetMenu(LoadMenu);
  351.   glutAddMenuEntry("-> Rescan Directory <-", -1);
  352.   glutSetMenu(SaveMenu);
  353.   CSetNames[csetnum] = "NewCurve0";
  354.   glutAddMenuEntry("NewCurve0", csetnum++);
  355.   CSetNames[csetnum] = "NewCurve1";
  356.   glutAddMenuEntry("NewCurve1", csetnum++);
  357.   CSetNames[csetnum] = "NewCurve2";
  358.   glutAddMenuEntry("NewCurve2", csetnum++);
  359.  
  360.   return (csetnum - 2);  /* just indicate curves in Load menu */
  361. }
  362.  
  363.  
  364. /***************************************************************/
  365. /********************* DISPLAY LISTS ***************************/
  366. /***************************************************************/
  367.  
  368.  
  369. void MakeCurveList(int joint)
  370. {
  371.   int i;
  372.  
  373.   glNewList(CurveLists+joint, GL_COMPILE);
  374.   glColor3f(1, 1, 1);
  375.   for (i = 0; i < CYCLE_SIZE; i++) {
  376.     glVertex3f((GLfloat)i/CYCLE_SIZE, Walk_cycle[0][joint][i]/180, 0);
  377.   }
  378.   glEndList();
  379. }
  380.  
  381. void MakeCPointList(int joint)
  382. {
  383.   int point;
  384.  
  385.   glNewList(ControlPtsLists+joint, GL_COMPILE);
  386.  
  387.     glColor3f(0, 0.4, 0);
  388.     glBegin(GL_LINE_STRIP);
  389.     for (point = 0; point < RotCurve[joint].numpoints; point++) {
  390.       if (!((point-2) % 3)) {
  391.         glEnd();
  392.         glBegin(GL_LINE_STRIP);
  393.       }
  394.       glVertex3f(RotCurve[joint].xcoords[point],
  395.              (RotCurve[joint].angles[point])/180.0, 0.0);
  396.     }
  397.     glEnd();
  398.  
  399.     glBegin(GL_POINTS);
  400.     for (point = 0; point < RotCurve[joint].numpoints; point++) {
  401.       if (point % 3)
  402.         glColor3f(0, 0.7, 0);
  403.       else
  404.         glColor3f(0.7, 0.0, 0);
  405.       glVertex3f(RotCurve[joint].xcoords[point],
  406.              (RotCurve[joint].angles[point])/180, 0);
  407.     }
  408.     glEnd();
  409.  
  410.   glEndList();
  411. }
  412.  
  413. void MakeJointLists(int joint)
  414. {
  415.   MakeCurveList(joint);
  416.   MakeCPointList(joint);
  417. }
  418.  
  419.  
  420. void ComputeCSetAndMakeLists(void)
  421. {
  422.   int joint;
  423.  
  424.   for(joint = 0; joint < NUM_JOINTS; joint++) {
  425.     ComputeCurve(joint);
  426.     MakeJointLists(joint);
  427.   }
  428. }
  429.  
  430. void MakeLists(void)
  431. {
  432.   HorizontalList = glGenLists(1);
  433.   glNewList(HorizontalList, GL_COMPILE);
  434.   {
  435.     float line1 = 25.0/180,
  436.           line2 = 35.0/180,
  437.           line3 = 45.0/180;
  438.     glColor3f(0, 0, 0.7);
  439.     glPushAttrib(GL_ENABLE_BIT);
  440.     glEnable(GL_LINE_STIPPLE);
  441.     glBegin(GL_LINES);
  442.       glVertex3f(0, 0, 0.5);
  443.       glVertex3f(1, 0, 0.5);
  444.       glVertex3f(0, line1, 0.5);  glVertex3f(1, line1, 0.5);
  445.       glVertex3f(0, -line1, 0.5); glVertex3f(1, -line1, 0.5);
  446.       glVertex3f(0, line2, 0.5);  glVertex3f(1, line2, 0.5);
  447.       glVertex3f(0, -line2, 0.5); glVertex3f(1, -line2, 0.5);
  448.       glVertex3f(0, line3, 0.5);  glVertex3f(1, line3, 0.5);
  449.       glVertex3f(0, -line3, 0.5); glVertex3f(1, -line3, 0.5);
  450.     glEnd();
  451.     glPopAttrib();
  452.   }
  453.   glEndList();
  454.  
  455.  
  456.   CurveLists = glGenLists(NUM_JOINTS);
  457.   assert(CurveLists != 0);
  458.  
  459.   ControlPtsLists = glGenLists(NUM_JOINTS);
  460.   assert(ControlPtsLists != 0);
  461.  
  462.   ComputeCSetAndMakeLists();
  463. }
  464.  
  465. /***************************************************************/
  466. /********************* curve WINDOW ****************************/
  467. /***************************************************************/
  468.  
  469. void CurveReshape(int w, int h)
  470. {
  471.   glViewport(0,0,w,h);
  472.   CurveWWidth = w; 
  473.   CurveWHeight = h;
  474.   glFlush();
  475. }
  476.  
  477. void CurveDisplay(void)
  478. {
  479.   int joint, otherlegstep;
  480.  
  481.   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  482.  
  483.   glPushMatrix();
  484.   glTranslatef(0, 0.5, 0);
  485.  
  486.   for (joint = NUM_JOINTS-1; joint >= 0; joint--) {
  487.  
  488.     if (DrawHorizontals) 
  489.       glCallList(HorizontalList);
  490.  
  491.     (CurveAsPoints) ?
  492.     glBegin(GL_POINTS) :
  493.     glBegin(GL_LINE_STRIP);
  494.       glCallList(CurveLists+joint);
  495.     glEnd();
  496.  
  497.     if (joint == EditingCurve) { 
  498.       glPointSize(5.0);
  499.       glCallList(ControlPtsLists+EditingCurve);
  500.       glPointSize(1.0);
  501.     }
  502.   glTranslatef(0, 1, 0);
  503.   }
  504.  
  505.   glPopMatrix();
  506.  
  507.   otherlegstep = (Step+50) % CYCLE_SIZE;
  508.     /* draw vertical line */
  509.   glColor3f(1, 1, 1);
  510.   glBegin(GL_LINES);
  511.     glVertex3f((GLfloat)Step/CYCLE_SIZE, 0, 0);
  512.     glVertex3f((GLfloat)Step/CYCLE_SIZE, NUM_JOINTS, 0);
  513.     if (!MirrorLegs) {
  514.       glVertex3f((GLfloat)otherlegstep/CYCLE_SIZE, 0, 0);
  515.       glVertex3f((GLfloat)otherlegstep/CYCLE_SIZE, NUM_JOINTS, 0);
  516.     }
  517.   glEnd();
  518.  
  519.   glFlush();
  520.   glutSwapBuffers();
  521. }
  522.  
  523.  
  524. void CurveGLInit(void)
  525. {
  526.   glMatrixMode(GL_PROJECTION);
  527.   glLoadIdentity();
  528.   glOrtho(0,1,0,NUM_JOINTS,1,-1);
  529.  
  530.   glMatrixMode(GL_MODELVIEW);
  531.   glLoadIdentity();
  532.  
  533.   glLineStipple(1, 0x00FF);
  534.  
  535.   glEnable(GL_DEPTH_TEST);
  536.   glDepthFunc(GL_LEQUAL);
  537.  
  538.  
  539.   glFlush();
  540.  
  541. } 
  542.  
  543.  
  544.  
  545. void CurveHandleButton(int button, int state, int x, int y)
  546. {
  547.   if (button == GLUT_RIGHT_BUTTON )
  548.     return;
  549.  
  550.   if (state == GLUT_DOWN && CurveDownBtn == -1) {
  551.     if (button == GLUT_MIDDLE_BUTTON)
  552.       CurveHandleMenu(CMENU_WALK);
  553.     else
  554.       fStep = Step = (int)((float)x/glutGet(GLUT_WINDOW_WIDTH) * CYCLE_SIZE);
  555.       
  556.     CurveDownBtn = button;
  557.  
  558.     WasWalking = Walking;
  559.     StopWalking();
  560.     RedisplayBoth();
  561.   } else if (button == CurveDownBtn) {
  562.     CurveDownBtn = -1;
  563.     if (WasWalking) {
  564.       Walking = 1;
  565.       agvSetAllowIdle(0);
  566.       glutIdleFunc(IncrementStep);    
  567.     }
  568.   }
  569. }
  570.  
  571. float CurveEditConstrain(float fx)
  572. {
  573.   if (CurvePickedPoint == 0)
  574.     fx = 0;
  575.   else if (CurvePickedPoint == RotCurve[EditingCurve].numpoints-1)
  576.     fx = 1;
  577.   else if (!(CurvePickedPoint % 3)) {  /* is a pivot */
  578.     fx = MAX(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint-1]);
  579.     fx = MIN(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint+1]);   
  580.     fx = MAX(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint-3]);
  581.     fx = MIN(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint+3]);       
  582.   } else if (!((CurvePickedPoint - 1) % 3)) {   /* is right slope */
  583.     fx = MAX(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint-1]);
  584.   } else {
  585.     fx = MIN(fx, RotCurve[EditingCurve].xcoords[CurvePickedPoint+1]);
  586.   } 
  587.   return fx;
  588. }
  589.  
  590. void RemovePoint(int pt)
  591. {
  592.   int i;
  593.  
  594.   for (i = pt - 1; i < RotCurve[EditingCurve].numpoints; i++) {
  595.     RotCurve[EditingCurve].xcoords[i] = RotCurve[EditingCurve].xcoords[i+3];
  596.     RotCurve[EditingCurve].angles[i]  = RotCurve[EditingCurve].angles[i+3];
  597.   }
  598.   RotCurve[EditingCurve].numpoints -= 3;
  599. }
  600.  
  601. void AddPoint(float fx)
  602. {
  603.   int i, j;
  604.  
  605.   if (fx < 0.05 || fx > 0.95 || RotCurve[EditingCurve].numpoints + 3 >=
  606.                                 MAX_CPOINTS)
  607.     return;
  608.  
  609.   for (i = 3; i < RotCurve[EditingCurve].numpoints; i += 3) {
  610.     if (fx < RotCurve[EditingCurve].xcoords[i]) {
  611.       for (j = RotCurve[EditingCurve].numpoints + 2; j > i + 1; j--) { 
  612.         RotCurve[EditingCurve].xcoords[j] =
  613.                 RotCurve[EditingCurve].xcoords[j-3];
  614.         RotCurve[EditingCurve].angles[j] =
  615.                 RotCurve[EditingCurve].angles[j-3];
  616.       }
  617.     RotCurve[EditingCurve].xcoords[i]   = fx;
  618.     RotCurve[EditingCurve].angles[i]    =
  619.       Walk_cycle[0][EditingCurve][(int)(fx*CYCLE_SIZE)];
  620.     RotCurve[EditingCurve].xcoords[i-1] = fx - 0.05;
  621.     RotCurve[EditingCurve].angles[i-1]  =
  622.       Walk_cycle[0][EditingCurve][(int)((fx-0.05)*CYCLE_SIZE)];
  623.     RotCurve[EditingCurve].xcoords[i+1] = fx + 0.05;
  624.     RotCurve[EditingCurve].angles[i+1]  = 
  625.       Walk_cycle[0][EditingCurve][(int)((fx+0.05)*CYCLE_SIZE)];
  626.     RotCurve[EditingCurve].numpoints += 3;
  627.     break;
  628.     }
  629.   }  
  630. }
  631.  
  632.  
  633. void CurveEditHandleButton(int button, int state, int x, int y)
  634. {
  635.   float fx, fy;
  636.   int point;
  637.  
  638.   fy = -(((float)y - ((float)CurveWHeight/NUM_JOINTS * EditingCurve)) /
  639.        ((float)CurveWHeight/NUM_JOINTS) - 0.5) * 180.0,
  640.   fx = (float)x/CurveWWidth;
  641.   
  642.   if (state == GLUT_DOWN && button == GLUT_LEFT_BUTTON &&
  643.                             CurveDownBtn == -1) {
  644.     CurvePickedPoint = -1;
  645.     
  646.     for (point = 0; point < RotCurve[EditingCurve].numpoints; point++) {
  647.       if (fabs(RotCurve[EditingCurve].xcoords[point] - fx) < 0.01 &&
  648.       fabs(RotCurve[EditingCurve].angles[point] - fy) < 4) {
  649.     CurvePickedPoint = point;
  650.     CurveLastX = x;
  651.     CurveLastY = y;
  652.     glutIdleFunc(CurveCPointDrag);
  653.     break;
  654.       }
  655.     }
  656.    if (CurvePickedPoint == -1)
  657.      CurveHandleButton(button, state, x, y);
  658.     CurveDownBtn = button;
  659.  
  660.  
  661.   } else if (state == GLUT_DOWN && button == GLUT_MIDDLE_BUTTON &&
  662.                                    CurveDownBtn == -1) {
  663.  
  664.     for (point = 3; point < RotCurve[EditingCurve].numpoints - 1; point += 3) {
  665.       if (fabs(RotCurve[EditingCurve].xcoords[point] - fx) < 0.01 &&
  666.       fabs(RotCurve[EditingCurve].angles[point] - fy) < 4) {
  667.     break;
  668.       }
  669.     }
  670.     if (point >= 3 && point < RotCurve[EditingCurve].numpoints - 1)
  671.       RemovePoint(point);
  672.     else if (fabs(Walk_cycle[0][EditingCurve][(int)(fx*CYCLE_SIZE)] - fy) < 4)
  673.       AddPoint(fx);
  674.     ComputeCurve(EditingCurve);
  675.     MakeJointLists(EditingCurve);    
  676.     RedisplayBoth();
  677.  
  678.   } else if (button == GLUT_LEFT_BUTTON && button == CurveDownBtn) {
  679.  
  680.     y = MAX(y, 0); y = MIN(y, CurveWHeight);
  681.     x = MAX(x, 0); x = MIN(x, CurveWWidth);
  682.     fy = -(((float)y - ((float)CurveWHeight/NUM_JOINTS * EditingCurve)) /
  683.          ((float)CurveWHeight/NUM_JOINTS) - 0.5) * 180.0,
  684.     fx = (float)x/CurveWWidth;
  685.     CurveDownBtn = -1;
  686.     if (CurvePickedPoint != -1) {
  687.       fx = CurveEditConstrain(fx);
  688.       RotCurve[EditingCurve].xcoords[CurvePickedPoint] = fx;
  689.       RotCurve[EditingCurve].angles[CurvePickedPoint] = fy;        
  690.       ComputeCurve(EditingCurve);
  691.       MakeJointLists(EditingCurve);
  692.       glutIdleFunc(NULL);
  693.       RedisplayBoth();
  694.     }
  695.   }
  696. }  
  697.  
  698.  
  699. void CurveHandleMotion(int x, int y)
  700. {
  701.   if (CurvePickedPoint == -1) { 
  702.     
  703.     if (CurveDownBtn == GLUT_LEFT_BUTTON || CurveDownBtn ==
  704.                                             GLUT_MIDDLE_BUTTON) {
  705.       Step = (int)((float)x/glutGet(GLUT_WINDOW_WIDTH) * CYCLE_SIZE)
  706.     % CYCLE_SIZE;
  707.       if (Step < 0)
  708.     Step = CYCLE_SIZE + Step;
  709.       fStep = Step;
  710.  
  711.     RedisplayBoth();
  712.     }
  713.   } else {
  714.     y = MAX(y, 0); y = MIN(y, CurveWHeight);
  715.     x = MAX(x, 0); x = MIN(x, CurveWWidth);
  716.     CurveLastX = x;
  717.     CurveLastY = y;
  718.   }
  719. }
  720.  
  721. void CurveCPointDrag(void)
  722. {
  723.   float fx, fy;
  724.  
  725.   if (CurveDownBtn == GLUT_LEFT_BUTTON && CurvePickedPoint != -1) {
  726.     fy = -(((float)CurveLastY -
  727.            ((float)CurveWHeight/NUM_JOINTS * EditingCurve)) /
  728.           ((float)CurveWHeight/NUM_JOINTS) - 0.5) * 180.0,
  729.     fx = (float)CurveLastX/CurveWWidth;
  730.  
  731.     fx = CurveEditConstrain(fx);
  732.     RotCurve[EditingCurve].xcoords[CurvePickedPoint] = fx;
  733.     RotCurve[EditingCurve].angles[CurvePickedPoint] = fy;        
  734.     ComputeCurve(EditingCurve);
  735.     MakeJointLists(EditingCurve);
  736.     RedisplayBoth();
  737.   }
  738. }
  739.  
  740. void CurveHandleKeys(unsigned char key, int x, int y)
  741. {
  742.   if (key > '0' && key < '9')
  743.     CurveHandleEditMenu((key-'0')-1);
  744.   else if (key == 'd')
  745.     CurveHandleMenu(CMENU_DONEEDIT);
  746.   else {
  747.     switch(key) {
  748.       case 'f':
  749.       case ' ': Step++;
  750.                 StopWalking(); break;
  751.       case 'F': Step += 5;
  752.                 StopWalking(); break;
  753.       case 'b': Step--;
  754.                 StopWalking(); break;
  755.       case 'B': Step -= 5; 
  756.                 StopWalking(); break;
  757.     }
  758.     Step %= CYCLE_SIZE;
  759.     if (Step < 0)
  760.       Step = CYCLE_SIZE + Step;
  761.     fStep = Step;
  762.     RedisplayBoth();
  763.   }
  764. }
  765.  
  766. void CurveHandleEditMenu(int curve)
  767. {
  768.   if (curve >= NUM_JOINTS)
  769.     return;
  770.   if (EditingCurve == -1) {
  771.     WasWalking = Walking;
  772.     Walking = 0;
  773.     agvSetAllowIdle(0);   /* don't allow spinning, just slows us down */
  774.     glutIdleFunc(NULL);
  775.     glutMouseFunc(CurveEditHandleButton);
  776.   }
  777.   EditingCurve = curve;
  778.   glutPostRedisplay();
  779. }
  780.  
  781. void CurveHandleSZMenu(int size)
  782. {
  783.   IncStep = (float)size/100;
  784. }
  785.  
  786. void CurveHandleMenu(int value)
  787. {
  788.   switch (value) {
  789.     case CMENU_QUIT:
  790.       exit(0);
  791.       break;
  792.     case CMENU_CURVE:
  793.       CurveAsPoints = !CurveAsPoints;
  794.       glutPostRedisplay();
  795.       break;
  796.     case CMENU_HORIZ:
  797.       DrawHorizontals = !DrawHorizontals;
  798.       glutPostRedisplay();
  799.       break;
  800.     case CMENU_WALK:
  801.       if (EditingCurve != -1)
  802.     break;
  803.       Walking = !Walking;
  804.       if (Walking) {
  805.         agvSetAllowIdle(0);
  806.     glutIdleFunc(IncrementStep);    
  807.       } else {
  808.         agvSetAllowIdle(1);
  809.       }
  810.       break;
  811.     case CMENU_DONEEDIT:
  812.       glutMouseFunc(CurveHandleButton);
  813.       EditingCurve = -1;
  814.       CurvePickedPoint = -1;
  815.       Walking = WasWalking;
  816.       if (Walking)
  817.     glutIdleFunc(IncrementStep);
  818.       else
  819.         agvSetAllowIdle(1);
  820.       glutPostRedisplay();
  821.       break;
  822.     case CMENU_RESET:
  823.       FlatCSet();
  824.       ComputeCSetAndMakeLists();
  825.       glutPostRedisplay();
  826.       break;
  827.     case CMENU_MIRROR:
  828.       MirrorLegs = !MirrorLegs;
  829.       ComputeCSetAndMakeLists();
  830.       glutPostRedisplay();      
  831.     }
  832. }
  833.  
  834. void CurveMenuInit(void)
  835. {
  836.   int i;
  837.   char label[3];
  838.  
  839.   if (CurveEditMenu != -1) {
  840.     glutDestroyMenu(CurveEditMenu);
  841.     glutDestroyMenu(CurveMenu);
  842.     glutDestroyMenu(StepSizeMenu);
  843.   }
  844.  
  845.   CurveEditMenu = glutCreateMenu(CurveHandleEditMenu);
  846.   for (i = 0; i < NUM_JOINTS; i++) {
  847.     sprintf(label, " %d ", i+1);
  848.     glutAddMenuEntry(label, i);
  849.   }
  850.   StepSizeMenu = glutCreateMenu(CurveHandleSZMenu);
  851.   glutAddMenuEntry("0.25", 25);
  852.   glutAddMenuEntry("0.5",  50);
  853.   glutAddMenuEntry("1.0", 100);
  854.   glutAddMenuEntry("2.0", 200);
  855.   glutAddMenuEntry("3.0", 300);
  856.   glutAddMenuEntry("5.0", 500);
  857.   CurveMenu = glutCreateMenu(CurveHandleMenu);
  858.   glutAddSubMenu("Load Curve Set", LoadMenu);
  859.   glutAddSubMenu("Save As Curve Set", SaveMenu);
  860.   glutAddSubMenu("Edit Curve", CurveEditMenu);
  861.   glutAddMenuEntry("Done Editing", CMENU_DONEEDIT);
  862.   glutAddMenuEntry("Flatten Curve Set", CMENU_RESET);
  863.   glutAddMenuEntry("Toggle mirrored", CMENU_MIRROR);
  864.   glutAddSubMenu("Step size", StepSizeMenu);
  865.   glutAddMenuEntry("Toggle dotted", CMENU_CURVE);
  866.   glutAddMenuEntry("Toggle horizontals", CMENU_HORIZ);
  867.   glutAddMenuEntry("Toggle walking", CMENU_WALK);
  868.   glutAddMenuEntry("Quit", CMENU_QUIT);
  869.   glutAttachMenu(GLUT_RIGHT_BUTTON);
  870. }
  871.  
  872. void CurveVisible(int v)
  873. {
  874.   if (v == GLUT_VISIBLE)
  875.     CurveWindowVisible = 1;
  876.   else 
  877.     CurveWindowVisible = 0;
  878. }
  879.  
  880. /***************************************************************/
  881. /*********************** GUY WINDOW ****************************/
  882. /***************************************************************/
  883.  
  884. void GuyDisplay(void)
  885. {
  886.   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  887.   glFlush();
  888.  
  889.   glLoadIdentity();
  890.  
  891.   agvViewTransform();
  892.  
  893.   if (DrawAxes)
  894.     glCallList(AxesList);
  895.  
  896.   switch(GuyModel) {
  897.     case WIRECUBE:   DrawTheGuy_WC();  break;
  898.     case SOLIDCUBE:  DrawTheGuy_SC();  break;
  899.     case CYLINDER1:  DrawTheGuy_SL();  break;
  900.     case CYLINDER2:  DrawTheGuy_SL2(); break;
  901.   }
  902.  
  903.   glutSwapBuffers();
  904.   glFlush();
  905. }
  906.  
  907. void GuyReshape(int w, int h)
  908. {
  909.   glViewport(0,0,w,h);
  910.   glMatrixMode(GL_PROJECTION);
  911.   glLoadIdentity();
  912.   if (ViewPerspective)
  913.     gluPerspective(60.0, (GLdouble)w/h, 0.01, 100);
  914.   else
  915.     glOrtho(-1.2, 1.2, -1.2, 1.2, 0.1, 100);
  916.   glMatrixMode(GL_MODELVIEW);
  917.   glFlush();
  918. }
  919.  
  920.  
  921. void GuyGLInit(void)
  922. {
  923.   GLfloat mat_ambient[] = { 1.0, 1.0, 1.0, 1.0 };
  924.   GLfloat light_position[] = { 0.3, 0.5, 0.8, 0.0 };
  925.   GLfloat lm_ambient[] = { 0.2, 0.2, 0.2, 1.0 };
  926.   
  927.   glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
  928.   glLightfv(GL_LIGHT0, GL_POSITION, light_position);
  929.   glLightModelfv(GL_LIGHT_MODEL_AMBIENT, lm_ambient);
  930.   
  931.   glEnable(GL_LIGHTING);
  932.   glEnable(GL_LIGHT0);
  933.   glDepthFunc(GL_LESS);
  934.   glEnable(GL_DEPTH_TEST);
  935.   glEnable(GL_NORMALIZE);
  936.   glShadeModel(GL_SMOOTH);
  937.   /* Cylinder stuff */
  938.   StoreTheGuy_SL();
  939.   StoreTheGuy_SL2();
  940. }
  941.  
  942. void GuyHandleKeys(unsigned char key, int x, int y)
  943. {
  944.   switch(key) {
  945.     case 'f':
  946.     case ' ': Step++;
  947.               StopWalking(); break;
  948.     case 'F': Step += 5;
  949.               StopWalking(); break;
  950.     case 'b': Step--;
  951.               StopWalking(); break;
  952.     case 'B': Step -= 5; 
  953.               StopWalking(); break;
  954.   }
  955.   Step %= CYCLE_SIZE;
  956.   if (Step < 0)
  957.     Step = CYCLE_SIZE + Step;
  958.   agvHandleKeys(key, x, y);
  959.   RedisplayBoth();
  960. }
  961.  
  962. typedef enum { GMENU_QUIT, GMENU_CURVE, GMENU_HORIZ,
  963.                GMENU_AXES, GMENU_PERSP } GuyMenuChoices;
  964.  
  965.  
  966. void GuyModelHandleMenu(int model)
  967. {
  968.   GuyModel = model;
  969.   if (model == WIRECUBE)
  970.     glDisable(GL_LIGHTING);
  971.   else
  972.     glEnable(GL_LIGHTING);
  973.   glutPostRedisplay();
  974. }
  975.  
  976. void GuyHandleMenu(int value)
  977. {
  978.   switch (value) {
  979.     case GMENU_QUIT:
  980.       exit(0);
  981.       break;
  982.     case GMENU_AXES:
  983.       DrawAxes = !DrawAxes;
  984.       glutPostRedisplay();
  985.       break;
  986.     case GMENU_PERSP:
  987.       ViewPerspective = !ViewPerspective;
  988.       GuyReshape(glutGet(GLUT_WINDOW_WIDTH), glutGet(GLUT_WINDOW_HEIGHT));
  989.       glutPostRedisplay();
  990.       break;
  991.   }
  992. }
  993.  
  994. void GuyMenuInit(void)
  995. {
  996.   int sub2, sub1 = glutCreateMenu(agvSwitchMoveMode);
  997.   glutAddMenuEntry("Flying move",  FLYING);
  998.   glutAddMenuEntry("Polar move",   POLAR);
  999.   sub2 = glutCreateMenu(GuyModelHandleMenu);
  1000.   glutAddMenuEntry("Wire cubes",  WIRECUBE);
  1001.   glutAddMenuEntry("Solid cubes", SOLIDCUBE);
  1002.   glutAddMenuEntry("Cylinder 1",  CYLINDER1);
  1003.   glutAddMenuEntry("Cylinder 2",  CYLINDER2);
  1004.   glutCreateMenu(GuyHandleMenu);
  1005.   glutAddSubMenu("Viewing", sub1);
  1006.   glutAddSubMenu("Model", sub2);
  1007.   glutAddMenuEntry("Toggle Axes",    GMENU_AXES);
  1008.   glutAddMenuEntry("Toggle Perspective View", GMENU_PERSP);
  1009.   glutAddMenuEntry("Quit",           GMENU_QUIT);
  1010.   glutAttachMenu(GLUT_RIGHT_BUTTON);
  1011. }
  1012.  
  1013. /***************************************************************/
  1014. /********************* BOTH WINDOWS ****************************/
  1015. /***************************************************************/
  1016.  
  1017. void RedisplayBoth(void)
  1018. {
  1019.   glutSetWindow(GuyWindow);
  1020.   glutPostRedisplay();
  1021.   if (CurveWindowVisible) {
  1022.     glutSetWindow(CurveWindow);
  1023.     glutPostRedisplay();
  1024.   }
  1025. }
  1026.  
  1027. void IncrementStep(void)
  1028. {
  1029.   fStep = fmod(fStep + IncStep, CYCLE_SIZE);
  1030.   Step = (int)fStep;
  1031.   if (agvMoving)
  1032.     agvMove();
  1033.   RedisplayBoth();
  1034. }
  1035.  
  1036. void StopWalking(void)
  1037. {
  1038.   if (Walking) {
  1039.     Walking = 0;
  1040.     agvSetAllowIdle(1);
  1041.   }
  1042. }
  1043.  
  1044. void SetWindowTitles(char *csetname)
  1045. {
  1046.   char windowtitle[MAX_CSETNAMELEN + 20];
  1047.  
  1048.   strcpy(windowtitle, "Rotation Curves: ");
  1049.   strcat(windowtitle, csetname);
  1050.   glutSetWindow(CurveWindow);
  1051.   glutSetWindowTitle(windowtitle);
  1052.  
  1053.   strcpy(windowtitle, "The Guy: ");
  1054.   strcat(windowtitle, csetname);
  1055.   glutSetWindow(GuyWindow);
  1056.   glutSetWindowTitle(windowtitle);
  1057. }
  1058.  
  1059. /***************************************************************/
  1060. /***************************** MAIN ****************************/
  1061. /***************************************************************/
  1062.  
  1063. void main(int argc, char** argv)
  1064. {
  1065.   glutInit(&argc, argv);
  1066.  
  1067.   glutInitWindowSize(512, 512);
  1068.   glutInitWindowPosition(700, 250);
  1069.   glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  1070.   GuyWindow = glutCreateWindow("The Guy:");
  1071.   agvInit(!Walking);
  1072.   AxesList = glGenLists(1);
  1073.   agvMakeAxesList(AxesList);
  1074.   GuyGLInit();
  1075.   GuyMenuInit();
  1076.   glutDisplayFunc(GuyDisplay);
  1077.   glutReshapeFunc(GuyReshape);
  1078.   glutKeyboardFunc(GuyHandleKeys);
  1079.  
  1080.   glutInitWindowSize(512, 1024);
  1081.   glutInitWindowPosition(100, 0);
  1082.   glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
  1083.   CurveWindow = glutCreateWindow("Rotation Curves:");
  1084.   CurveGLInit();
  1085.   glutDisplayFunc(CurveDisplay);
  1086.   glutReshapeFunc(CurveReshape);
  1087.   glutMouseFunc(CurveHandleButton);
  1088.   glutMotionFunc(CurveHandleMotion);
  1089.   glutKeyboardFunc(CurveHandleKeys);
  1090.   glutVisibilityFunc(CurveVisible);
  1091.  
  1092.   FlatCSet();
  1093.   MakeLists();
  1094.  
  1095.   if (MakeLoadAndSaveMenus() > 0)  /* read first curve if there was one */
  1096.     HandleLoadMenu(0);
  1097.  
  1098.   CurveMenuInit();
  1099.  
  1100.   glutMainLoop();
  1101. }
  1102.  
  1103.  
  1104.  
  1105.  
  1106.